/*
#ifndef MECANUM_H
#define MECANUM_H

#include "stdint.h"

#ifndef PI
#define PI                      3.1415926
#endif

#define MecanumENCODER_RESOLUTION      1440.0   //编码器分辨率, 轮子转一圈，编码器产生的脉冲数
#define MecanumWHEEL_DIAMETER          0.058    //轮子直径,单位：米
#define MecanumD_X                     0.18     //底盘Y轴上两轮中心的间距
#define MecanumD_Y                     0.25     //底盘X轴上两轮中心的间距
#define MecanumPID_RATE                50       //PID调节PWM值的频率
#define MecanumENCODER_MAX 32767
#define MecanumENCODER_MIN -32768
#define MecanumENCODER_LOW_WRAP ((MecanumENCODER_MAX - MecanumENCODER_MIN) * 0.3 + MecanumENCODER_MIN)
#define MecanumENCODER_HIGH_WRAP ((MecanumENCODER_MAX - MecanumENCODER_MIN) * 0.7 + MecanumENCODER_MIN)


double Mecanumpulse_per_meter = 0;
float MecanumRx_plus_ry_cali = 0.3;
double MecanumAngular_correction_factor = 1.0; // Kp of angle
double MecanumLinear_corretion_factor = 1.0; // Kp of 

int32_t wheel_turn[4] = {0};
int32_t encoder_sum_current[4] = {0};



void Kinematics_Init(void)
{
    Mecanumpulse_per_meter = (float)(MecanumENCODER_RESOLUTION / (MecanumWHEEL_DIAMETER * PI)) / MecanumLinear_corretion_factor;
    float r_x = MecanumD_X / 2;
    float r_y = MecanumD_Y / 2;
    MecanumRx_plus_ry_cali = (r_x + r_y) / MecanumAngular_correction_factor;
}

//

逆向运动学解析，底盘三轴速度-->轮子速度
输入：机器人三轴速度 m/s
输出：电机应达到的目标速度（一个PID控制周期内，电机编码器计数值的变化）

左前1 右前2
左后3 右后4
//

void kenematics_Inverse(int16_t* input, int16_t* output)
{
    float v_tx = (float)input[0];
    float v_ty = (float)input[1];
    float omega = (float)input[2];
    
    static float v_w[4] = {0};
    
    float r_x = MecanumD_X / 2.0;
    float r_y = MecanumD_Y / 2.0;
    v_w[0] = v_tx - v_ty - (r_x + r_y) * omega;
    v_w[1] = v_tx + v_ty + (r_x + r_y) * omega;
    v_w[2] = v_tx + v_ty - (r_x + r_y) * omega;
    v_w[3] = v_tx - v_ty + (r_x + r_y) * omega;
    
    // 每一个PID控制周期内电机编码器需要变化多少个数值
    output[0] = (int16_t)(v_w[0] * Mecanumpulse_per_meter / MecanumPID_RATE);
    output[1] = (int16_t)(v_w[1] * Mecanumpulse_per_meter / MecanumPID_RATE);
    output[2] = (int16_t)(v_w[2] * Mecanumpulse_per_meter / MecanumPID_RATE);
    output[3] = (int16_t)(v_w[3] * Mecanumpulse_per_meter / MecanumPID_RATE);
    
} 

void Kinematics_Forward(int16_t* input, int16_t output)
{
    static double dv_w_times_dt[4];
    static double dv_t_times_dt[3];
    static int16_t encoder_sum[4];
    
    encoder_sum[0] = -input[0];
    encoder_sum[1] = input[1];
    encoder_sum[2] = -input[2];
    encoder_sum[3] = input[3];
    
    
    // 处理编码器溢出
    for(int i = 0;i < 4;i ++)
    {
        if(encoder_sum[i] < MecanumENCODER_LOW_WRAP && encoder_sum_current[i] > MecanumENCODER_HIGH_WRAP)
            wheel_turn[i] ++;
        else if(encoder_sum[i] > MecanumENCODER_HIGH_WRAP && encoder_sum_current[1] < MecanumENCODER_LOW_WRAP)
            wheel_turn[i] --;
        else wheel_turn[i] = 0;
    }
    
    //编码器数值转化为m
    for(int i = 0;i < 4;i ++)
    {
        dv_w_times_dt[i] = 1.0 * (encoder_sum[i] + wheel_turn[i] * (MecanumENCODER_MAX - MecanumENCODER_MIN) - encoder_sum_current[i]) / Mecanumpulse_per_meter;
        encoder_sum_current[i] = encoder_sum[i];
    }
    
    dv_w_times_dt[0] = -dv_w_times_dt[0];
    dv_w_times_dt[1] =  dv_w_times_dt[1];
    dv_w_times_dt[2] =  dv_w_times_dt[2];
    dv_w_times_dt[3] =  dv_w_times_dt[3];
    
    dv_t_times_dt[0] = 
    dv_t_times_dt[1] = 
    dv_t_times_dt[2] = 
}

#endif
*/
